/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: SpeedCtrlData.cpp,v 1.18 2009-07-22 14:49:14 morsko Exp $
 *
 * \file
 * \brief Implementation for SpeedCtrl interface data encoder/decoder.
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#include "SpeedCtrlData.hpp"
#include "binbag.h"
#include "owndebug.h"

//*****************************************************************************
using namespace MaCI::SpeedCtrl;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CSpeedCtrlData::CSpeedCtrlData(void)
  : iValid(false),
    iMaxSpeedLimitSet(false),
    iMinSpeedLimitSet(false), 
    iTimestampPtr(NULL),
    iCommandPtr(NULL),
    iSpeedPtr(NULL),
    iSpeedLimitsPtr(NULL),
    iCommandIntervalsPtr(NULL),
    iTimeoutTimePtr(NULL)
{
}
//*****************************************************************************

CSpeedCtrlData::~CSpeedCtrlData(void)
{
}
//****************************************************************************

CSpeedCtrlData::CSpeedCtrlData(const CSpeedCtrlData &aSpeedCtrlData)
  : iValid(false),
    iMaxSpeedLimitSet(false),
    iMinSpeedLimitSet(false),
    iTimestampPtr(NULL),
    iCommandPtr(NULL),
    iSpeedPtr(NULL),
    iSpeedLimitsPtr(NULL),
    iCommandIntervalsPtr(NULL),
    iTimeoutTimePtr(NULL)
{
  *this = aSpeedCtrlData;
}
//****************************************************************************

CSpeedCtrlData &CSpeedCtrlData::operator=(const CSpeedCtrlData &aSpeedCtrlData) 
{ 
  // Destroy the internal binbag (unconditional)
  DestroyInternalBinBag();
  Reset();
  // Create an internal BinBag instance
  if (CreateInternalBinBag(aSpeedCtrlData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aSpeedCtrlData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"WARNING: Failed to Decode SpeedCtrlData even though assignment operator used?");
  }

  return *this; 
}
//****************************************************************************

void CSpeedCtrlData::Reset()
{
  CMaCIData::Reset();

  iValid = false;
  iMaxSpeedLimitSet = false;
  iMinSpeedLimitSet = false;
  iTimestampPtr = NULL;
  iCommandPtr = NULL;
  iSpeedPtr = NULL;
  iSpeedLimitsPtr = NULL;
  iCommandIntervalsPtr= NULL;
  iTimeoutTimePtr= NULL;;
}
//*****************************************************************************

bool CSpeedCtrlData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer
  if (aBinBag != NULL)  
    iBinBagContainer = aBinBag;

  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeTimestamp:
	if (iTimestampPtr == NULL) {
	  iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);
	} else{ result = false;}
	break;
	
      case KTypeCommand:
	if (iCommandPtr == NULL) {
	  iCommandPtr = reinterpret_cast<const TCommand *>(b->data);

	} else{ result = false;}
	break;

      case KTypeSpeed:
	if (iSpeedPtr == NULL) {
	  iSpeedPtr = reinterpret_cast<const TSpeed *>(b->data);

	} else{ result = false;}
	break;

      case KTypeInterval:
	if (iCommandIntervalsPtr == NULL) {
	  iCommandIntervalsPtr = reinterpret_cast<const TCommandIntervals *>(b->data);

	} else {result = false;}
	break;

      case KTypeSpeedLimits:
        if(iSpeedLimitsPtr == NULL){
          iSpeedLimitsPtr = reinterpret_cast<const TSpeed * >(b->data);
          iMaxSpeedLimitSet = true;
        }else if(!iMinSpeedLimitSet){ 
          iMinSpeedLimitSet = true;
        }else{
          result = false;
        }
        break;

      case KTypeTimeoutTime:
        if(iTimeoutTimePtr == NULL){
          iTimeoutTimePtr = reinterpret_cast<const TTimeoutTime * >(b->data);
        }else {result = false;}
        break;

      default:
	// Unknown Type in BinBag. Do nothing. (Ignore for compability)
	break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else{ result = false;}
  
  // Store the result
  SetReadOnlyContainer( (iValid = result) );
  
  return result;
}
//*****************************************************************************

bool CSpeedCtrlData::SetTimestamp(const Common::TTimestamp &aStamp)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeTimestamp, 
                                  (const char *)&aStamp, sizeof(Common::TTimestamp));
    iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);
    
    // Mark last element
    SetLastElementType( KTypeTimestamp);
    
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CSpeedCtrlData::SetCommand(const TCommand &aCmd)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeCommand, (const char *)&aCmd, sizeof(TCommand));
    iCommandPtr = reinterpret_cast<const TCommand *>(b->data);

    // Mark last element
    SetLastElementType( KTypeCommand);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CSpeedCtrlData::SetSpeed(const TSpeed &aSpeed)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iSpeedPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeSpeed, (const char *)&aSpeed, sizeof(TSpeed));
    iSpeedPtr = reinterpret_cast<const TSpeed *>(b->data);

    // Mark last element
    SetLastElementType( KTypeSpeed);

    result = true;
  }
  return result;
}

//*****************************************************************************

bool CSpeedCtrlData::SetCommandIntervals(const TCommandIntervals &aCommandIntervals)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandIntervalsPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeInterval, (const char *)&aCommandIntervals, sizeof(TCommandIntervals));
    iCommandIntervalsPtr = reinterpret_cast<const TCommandIntervals *>(b->data);

    // Mark last element
    SetLastElementType( KTypeInterval);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CSpeedCtrlData::SetSpeedLimits(const TSpeed &aMaxSpeeds, const TSpeed &aMinSpeeds)
{
  bool result = false;
  if(!IsReadOnlyContainer() && iSpeedLimitsPtr == NULL){
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeSpeedLimits, (const char *)&aMaxSpeeds, sizeof(TSpeed));

    //  const gim::binbag::TBinBlob *b2 = 
    iBinBagContainer->AddBinary(KTypeSpeedLimits, (const char *)&aMinSpeeds, sizeof(TSpeed));


    iSpeedLimitsPtr = reinterpret_cast<const TSpeed *>(b->data);

    // Mark last element
    SetLastElementType(KTypeSpeedLimits);

    result = true;

  }

  return result;

}
//*****************************************************************************

bool CSpeedCtrlData::SetTimeoutTime(const TTimeoutTime &aTimeoutTime)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimeoutTimePtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeTimeoutTime, (const char *)&aTimeoutTime, sizeof(TTimeoutTime));
    iTimeoutTimePtr = reinterpret_cast<const TTimeoutTime *>(b->data);

    // Mark last element
    SetLastElementType( KTypeTimeoutTime);

    result = true;
  }
  return result;
}
//*****************************************************************************
const MaCI::Common::TTimestamp *CSpeedCtrlData::GetTimestamp(void) const
{
  return iTimestampPtr;
}
//*****************************************************************************

const TCommand *CSpeedCtrlData::GetCommand(void) const
{
  return iCommandPtr;
}
//*****************************************************************************

const TSpeed *CSpeedCtrlData::GetSpeed(void) const
{
  return iSpeedPtr;
}
//*****************************************************************************

const TCommandIntervals *CSpeedCtrlData::GetCommandIntervals(void) const
{
  return iCommandIntervalsPtr;
}
//*****************************************************************************

const TSpeed *CSpeedCtrlData::GetMaxSpeedLimit(void)  const
{

  return iSpeedLimitsPtr;
}
//*****************************************************************************

const TSpeed *CSpeedCtrlData::GetMinSpeedLimit(void)  const
{
  const TSpeed *elem = NULL;

  if(iSpeedLimitsPtr != NULL){

    const char *cptr =(const char *)iSpeedLimitsPtr;
    cptr += sizeof(TSpeed) + sizeof(gim::binbag::TBinBlob);

    // Now, typecast the address to Element.
      elem = (const TSpeed *)cptr;
  }

  return elem;

}
//*****************************************************************************

const TTimeoutTime *CSpeedCtrlData::GetTimeoutTime(void)  const
{

  return iTimeoutTimePtr;
}
//*****************************************************************************
void CSpeedCtrlData::Print(const int l) const
{
  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeTimestamp: {
	const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
	dPrint(l,"TTimestamp : %d, %d", 
	       ts->timestamp_s,
	       ts->timestamp_us);
	break;
      }

      case KTypeCommand: {
	const TCommand *ts = 
          reinterpret_cast<const TCommand *>(b->data);
	dPrint(l,"TCommand: %d", 
	       ts->cmd);
	break;
      }
	
      case KTypeSpeed: {
	const TSpeed *ts = 
          reinterpret_cast<const TSpeed *>(b->data);
	dPrint(l,"TSpeed: %f, %f, %f", 
	       ts->speed, ts->angularSpeed, ts->acceleration);
	break;
      }
      case KTypeInterval: {
        const TCommandIntervals *ci = 
          reinterpret_cast<const TCommandIntervals *>(b->data);
	dPrint(l,"TCommandIntervals: max: %u, min: %u", 
	       ci->maxInterval, ci->minInterval);
	break;

      }

      case KTypeSpeedLimits:{
        const TSpeed *sl =
          reinterpret_cast<const TSpeed*>(b->data);
        float maxS = sl->speed;
        float maxAS = sl->angularSpeed;
        float maxA = sl->acceleration;
        b = iBinBagContainer->GetNextBinary();
       
        sl = reinterpret_cast<const TSpeed*>(b->data);
        float minS =sl->speed;
        float minAS =  sl->angularSpeed;
        float minA = sl->acceleration;

        dPrint(l,"speed limits, max: %f, %f, %f, min: %f, %f, %f",
               maxS,maxAS,maxA,
               minS,minAS,minA);
        break;
      }

      case KTypeTimeoutTime: {
        const TTimeoutTime *to = 
          reinterpret_cast<const TTimeoutTime *>(b->data);
	dPrint(l,"TTimeoutTime: %u", 
	       to->timeoutTime);
	break;

      }
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
    }
  }
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_SPEEDCTRLDATA_STANDALONE_MAIN

// Assert sequences verify that the container is working correctly
// (== The data read matches the data wrote in)
#include <assert.h>

int main(void)
{
  bool r;
  using namespace MaCI::SpeedCtrl;
  gim::binbag::CBinBag bb;
  CSpeedCtrlData A;
  CSpeedCtrlData B;

  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  r = A.EncodeTo(&bb);
  assert(r == true);

  printf("Encode: Timestamp (1, 2)\n");
  r = A.SetTimestamp(MaCI::Common::TTimestamp(1, 2));
  assert(r == true);

  printf("Encode: SetCommand ( KCommandSetSpeed(%d) )\n", KCommandSetSpeed);
  r = A.SetCommand(TCommand(KCommandSetSpeed));
  assert(r == true);

  printf("Encode: SetSpeed( 1.0, 2.0, 3.0 )\n");
  r = A.SetSpeed(TSpeed(1.0, 2.0, 3.0));
  assert(r == true);

  printf("Encode: Set Command Intervals( 5, 10)\n");
  r = A.SetCommandIntervals(TCommandIntervals(5,10));
  assert(r == true);

  printf("Encode: Set Speed limits max( 0.5, 1.8, 0.2) min(-0.5,-1.8, 0.0)\n");
  r = A.SetSpeedLimits(TSpeed(0.5, 1.8, 0.2),TSpeed(-0.5,-1.8, 0.0));
  assert(r == true);

  printf("Encode: Set Timeout time(500)\n");
  r = A.SetTimeoutTime(TTimeoutTime(500));
  assert(r == true);

  printf("\nDecoding...\n");

  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: Timestamp = (%d, %d)\n", 
	 B.GetTimestamp()->timestamp_s, 
	 B.GetTimestamp()->timestamp_us);
  assert(B.GetTimestamp()->timestamp_s == 1);
  assert(B.GetTimestamp()->timestamp_us == 2);
  
  printf("Decode: GetCommand() = (%d)\n", 
	 B.GetCommand()->cmd);
  assert(B.GetCommand()->cmd == KCommandSetSpeed);

  printf("Decode: GetSpeed() = (%f, %f, %f)\n",
	 B.GetSpeed()->speed,
	 B.GetSpeed()->angularSpeed,
	 B.GetSpeed()->acceleration);
  assert(B.GetSpeed()->speed == 1.0);
  assert(B.GetSpeed()->angularSpeed == 2.0);
  assert(B.GetSpeed()->acceleration == 3.0);

  printf("Decode: GetCommandIntervals() = (%u, %u)\n",
	 B.GetCommandIntervals()->maxInterval,
	 B.GetCommandIntervals()->minInterval);
  assert(B.GetCommandIntervals()->maxInterval == 5);
  assert(B.GetCommandIntervals()->minInterval == 10);

  
  printf("Decode: GetSpeedLimits() = max (%f, %f, %f), min (%f, %f, %f)\n",
	 B.GetMaxSpeedLimit()->speed,
	 B.GetMaxSpeedLimit()->angularSpeed,
	 B.GetMaxSpeedLimit()->acceleration,
         B.GetMinSpeedLimit()->speed,
	 B.GetMinSpeedLimit()->angularSpeed,
	 B.GetMinSpeedLimit()->acceleration);
  assert(B.GetMaxSpeedLimit()->speed == 0.5f);
  assert(B.GetMaxSpeedLimit()->angularSpeed == 1.8f);
  assert(B.GetMaxSpeedLimit()->acceleration == 0.2f);
  assert(B.GetMinSpeedLimit()->speed == -0.5f);
  assert(B.GetMinSpeedLimit()->angularSpeed == -1.8f);
  assert(B.GetMinSpeedLimit()->acceleration == 0.0f);

  printf("Decode: GetTimeoutTime() = (%u)\n", 
	 B.GetTimeoutTime()->timeoutTime);
  assert(B.GetTimeoutTime()->timeoutTime == 500);

  printf("\nCopying and Re-Decoding...\n");
  CSpeedCtrlData C(B);
  printf("Decode: Timestamp = (%d, %d)\n", 
	 C.GetTimestamp()->timestamp_s, 
	 C.GetTimestamp()->timestamp_us);
  assert(C.GetTimestamp()->timestamp_s == 1);
  assert(C.GetTimestamp()->timestamp_us == 2);
  
  printf("Decode: GetCommand() = (%d)\n", 
	 C.GetCommand()->cmd);
  assert(C.GetCommand()->cmd == KCommandSetSpeed);

  printf("Decode: GetSpeed() = (%f, %f, %f)\n",
	 C.GetSpeed()->speed,
	 C.GetSpeed()->angularSpeed,
	 C.GetSpeed()->acceleration);
  assert(C.GetSpeed()->speed == 1.0);
  assert(C.GetSpeed()->angularSpeed == 2.0);
  assert(C.GetSpeed()->acceleration == 3.0);

  printf("Decode: GetCommandIntervals() = (%u, %u)\n",
	 C.GetCommandIntervals()->maxInterval,
	 C.GetCommandIntervals()->minInterval);
  assert(C.GetCommandIntervals()->maxInterval == 5);
  assert(C.GetCommandIntervals()->minInterval == 10);


  printf("Decode: GetSpeedLimits() = max (%f, %f, %f), min (%f, %f, %f)\n",
	 C.GetMaxSpeedLimit()->speed,
	 C.GetMaxSpeedLimit()->angularSpeed,
	 C.GetMaxSpeedLimit()->acceleration,
         C.GetMinSpeedLimit()->speed,
	 C.GetMinSpeedLimit()->angularSpeed,
	 C.GetMinSpeedLimit()->acceleration);
  assert(C.GetMaxSpeedLimit()->speed == 0.5f);
  assert(C.GetMaxSpeedLimit()->angularSpeed == 1.8f);
  assert(C.GetMaxSpeedLimit()->acceleration == 0.2f);
  assert(C.GetMinSpeedLimit()->speed == -0.5f);
  assert(C.GetMinSpeedLimit()->angularSpeed == -1.8f);
  assert(C.GetMinSpeedLimit()->acceleration == 0.0f);

  printf("Decode: GetTimeoutTime() = (%u)\n", 
	 C.GetTimeoutTime()->timeoutTime);
  assert(C.GetTimeoutTime()->timeoutTime == 500);

  printf("Test complete.\n");
  

}
#endif
//*****************************************************************************
//*****************************************************************************
